

#include "geometry_msgs/msg/twist.hpp"
#include "geometry_msgs/msg/twist_stamped.hpp"
// #include "std_msgs/msg/float64_multi_array.hpp"
// from std_msgs.msg import Float64MultiArray
#include <cmath>
#include <vector>
#include <iostream>

namespace four_wheel_steering_controller {
    class twist2four {
        using Twist = geometry_msgs::msg::TwistStamped;
    public:
        twist2four(/* args */);
        ~twist2four();

        int four_wheel_steering_derivation(Twist& cmd_msg, double wheel_velocity[4], double steering_radians[4]) {
            // 计算每个轮子的速度和转向角度
            // ...
            // cmd_msg.twist.linear.x;
            double linearx, lineary, angularz;
            linearx = cmd_msg.twist.linear.x;
            lineary = cmd_msg.twist.linear.y;
            angularz = cmd_msg.twist.angular.z;
            double v1x, v1y, v2x, v2y, v3x, v3y, v4x, v4y;

            v1x = linearx - angularz * wheel_track_ / 2.0;
            v1y = lineary + angularz * wheel_base_ / 2.0;
            v2x = linearx - angularz * wheel_track_ / 2.0;
            v2y = lineary - angularz * wheel_base_ / 2.0;
            v3x = linearx + angularz * wheel_track_ / 2.0;
            v3y = lineary - angularz * wheel_base_ / 2.0;
            v4x = linearx + angularz * wheel_track_ / 2.0;
            v4y = lineary + angularz * wheel_base_ / 2.0;
            // std::cout << linearx<<lineary<<angularz<< std::endl;
            // std::cout << "twist2four::v1x: " << v1x << " v1y: " << v1y << " v2x: " << v2x << " v2y: " << v2y << " v3x: " << v3x << " v3y: "<< v3y << " v4x: " << v4x << " v4y: " << v4y << std::endl;

            double front_left_wheel_velocity_ = sqrt(v1x * v1x + v1y * v1y);
            double rear_left_wheel_velocity_ = sqrt(v2x * v2x + v2y * v2y) ;
            double rear_right_wheel_velocity_ = sqrt(v3x * v3x + v3y * v3y) ;
            double front_right_wheel_velocity_ = sqrt(v4x * v4x + v4y * v4y);
            double front_left_steering_radians_ = atan2(v1y, v1x);
            double rear_left_steering_radians_ = atan2(v2y, v2x);
            double rear_right_steering_radians_ = atan2(v3y, v3x);
            double front_right_steering_radians_ = atan2(v4y, v4x);

            if (front_left_steering_radians_ < -M_PI / 2) {
                front_left_steering_radians_ = front_left_steering_radians_ + M_PI;
                front_left_wheel_velocity_ = -front_left_wheel_velocity_;
            }
            else  if (front_left_steering_radians_ > M_PI / 2) {
                front_left_steering_radians_ = front_left_steering_radians_ - M_PI;
                front_left_wheel_velocity_ = -front_left_wheel_velocity_;
            }
            if (rear_left_steering_radians_ < -M_PI / 2) {
                rear_left_steering_radians_ = rear_left_steering_radians_ + M_PI;
                rear_left_wheel_velocity_ = -rear_left_wheel_velocity_;
            }
            else  if (rear_left_steering_radians_ > M_PI / 2) {
                rear_left_steering_radians_ = rear_left_steering_radians_ - M_PI;
                rear_left_wheel_velocity_ = -rear_left_wheel_velocity_;
            }
            if (rear_right_steering_radians_ < -M_PI / 2) {
                rear_right_steering_radians_ = rear_right_steering_radians_ + M_PI;
                rear_right_wheel_velocity_ = -rear_right_wheel_velocity_;
            }
            else  if (rear_right_steering_radians_ > M_PI / 2) {
                rear_right_steering_radians_ = rear_right_steering_radians_ - M_PI;
                rear_right_wheel_velocity_ = -rear_right_wheel_velocity_;
            }
            if (front_right_steering_radians_ < -M_PI / 2) {
                front_right_steering_radians_ = front_right_steering_radians_ + M_PI;
                front_right_wheel_velocity_ = -front_right_wheel_velocity_;
            }
            else  if (front_right_steering_radians_ > M_PI / 2) {
                front_right_steering_radians_ = front_right_steering_radians_ - M_PI;
                front_right_wheel_velocity_ = -front_right_wheel_velocity_;
            }
            if (std::max({ front_left_wheel_velocity_,rear_left_wheel_velocity_,rear_right_wheel_velocity_,front_right_wheel_velocity_ }) > max_wheel_velocity_) {
                double max_velocity_ = std::max({ front_left_wheel_velocity_,rear_left_wheel_velocity_,rear_right_wheel_velocity_,front_right_wheel_velocity_ });
                front_left_wheel_velocity_ = max_wheel_velocity_ * front_left_wheel_velocity_ / max_velocity_;
                rear_left_wheel_velocity_ = max_wheel_velocity_ * rear_left_wheel_velocity_ / max_velocity_;
                rear_right_wheel_velocity_ = max_wheel_velocity_ * rear_right_wheel_velocity_ / max_velocity_;
                front_right_wheel_velocity_ = max_wheel_velocity_ * front_right_wheel_velocity_ / max_velocity_;
            }
            if (std::min({ front_left_wheel_velocity_,rear_left_wheel_velocity_,rear_right_wheel_velocity_,front_right_wheel_velocity_ }) < -max_wheel_velocity_
                ) {
                double min_velocity_ = std::min({ front_left_wheel_velocity_,rear_left_wheel_velocity_,rear_right_wheel_velocity_,front_right_wheel_velocity_ }
                );
                front_left_wheel_velocity_ = -max_wheel_velocity_ * front_left_wheel_velocity_ / min_velocity_;
                rear_left_wheel_velocity_ = -max_wheel_velocity_ * rear_left_wheel_velocity_ / min_velocity_;
                rear_right_wheel_velocity_ = -max_wheel_velocity_ * rear_right_wheel_velocity_ / min_velocity_;
                front_right_wheel_velocity_ = -max_wheel_velocity_ * front_right_wheel_velocity_ / min_velocity_;
            }
            wheel_velocity[0] =
                front_left_wheel_velocity_;
            wheel_velocity[1] =
                rear_left_wheel_velocity_;
            wheel_velocity[2] =
                rear_right_wheel_velocity_;
            wheel_velocity[3] =
                front_right_wheel_velocity_;
            steering_radians[0] =
                front_left_steering_radians_;
            steering_radians[1] =
                rear_left_steering_radians_;
            steering_radians[2] =
                rear_right_steering_radians_;
            steering_radians[3] =
                front_right_steering_radians_;

            // 返回成功或失败
            return 0;
        }
        void conf(double  wheel_base,
            double  wheel_track,
            double  wheel_radius,
            double  max_wheel_velocity,
            double  max_wheel_acceleration
            // double  max_steering_angle,
            // double  max_steering_radians,
            // double  min_steering_radians,
                // double  max_steering_acceleration
        ) {
            wheel_base_ = wheel_base;
            wheel_track_ = wheel_track;
            wheel_radius_ = wheel_radius;
            max_wheel_velocity_ = max_wheel_velocity;
            max_wheel_acceleration_ = max_wheel_acceleration;
            // max_steering_angle_  = max_steering_angle;
            // max_steering_radians_  = max_steering_radians;
            // min_steering_radians_  = min_steering_radians;
            // max_steering_acceleration_  = max_steering_acceleration;
        }
    private:
        /* data */
        double  wheel_base_;
        double  wheel_track_;
        double  wheel_radius_;
        double  max_wheel_velocity_;
        double  max_wheel_acceleration_;
        double  max_steering_angle_;
        double  max_steering_radians;
        double  min_steering_radians;
        double  max_steering_acceleration_;

        double front_left_wheel_velocity_;
        double front_right_wheel_velocity_;
        double rear_left_wheel_velocity_;
        double rear_right_wheel_velocity_;

        double front_left_steering_radians_;
        double front_right_steering_radians_;
        double rear_left_steering_radians_;
        double rear_right_steering_radians_;

        // std::shared_ptr<Twist> command_msg_;

        // std_msgs::msg::Float64MultiArray wheel_velocity_msg_;
        // std_msgs::msg::Float64MultiArray steering_radians_msg_;
        // std::vector<double> wheel_velocity_msg_;
        // std::vector<double> steering_radians_msg_;

        // double  wheel_velocity[4];
        // double steering_radians[4];
        // std::shared_ptr<rclcpp::Publisher<std_msgs::msg::Float64MultiArray>> wheel_velocity_publisher_;
        // std::shared_ptr<rclcpp::Publisher<std_msgs::msg::Float64MultiArray>> steering_publisher_;

    };

    twist2four::twist2four(/* args */) {

    }

    twist2four::~twist2four() {
    }






}















